﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Collections;
namespace learnCsharpwinform
{
    public enum Cartesian
    {
        x, y, z, rot, pitch, yaw, rotx, roty, rotz, Euler, RPY
    };
    public enum Transfrom { Increase, Decrease, Clock, AntiClock, World, Tool, Custom };
    public enum Solution { OK, Singular, OutOfRange, HighSpeed, Unkown };
    public class coordinate : IDisposable
    {
        public static int Num = 0;
        public static coordinate Tool;

        public static ArrayList List = new ArrayList();
        public double[] val = new double[] { 0, 0, 0, 0, 0, 0 };
        public string Name = "";

        public Cartesian Type
        {
            get { return _Type; }
            set
            {
                if (value != Cartesian.RPY && value != Cartesian.Euler)
                { MessageBox.Show("坐标系赋值有误"); }
                else
                {
                    _Type = value;
                }
            }
        }

        public static void Insert(int index, coordinate coor)
        {
            List.Insert(index, coor);
        }
        public static void Delete(int index)
        {
            Num--;
            List[index] = null;
            List.RemoveAt(index);


        }

        private Cartesian _Type;
        public double x
        {
            get { return val[0]; }
            set { val[0] = value; }
        }
        public double y
        {
            get { return val[1]; }
            set { val[1] = value; }
        }
        public double z
        {
            get { return val[2]; }
            set { val[2] = value; }
        }
        public double rot
        {
            get { return val[3]; }
            set { val[3] = value; }
        }
        public double pitch
        {
            get { return val[4]; }
            set { val[4] = value; }
        }
        public double yaw
        {
            get { return val[5]; }
            set { val[5] = value; }
        }

        public void Dispose()
        {

        }


        public coordinate(double x, double y, double z, double rot, double pitch, double yaw, Cartesian type)
        {
            val[0] = x; val[1] = y; val[2] = z; val[3] = rot; val[4] = pitch; val[5] = yaw;
            Type = type;
            Num++;
        }

        public coordinate(double[] para, Cartesian type)
        {
            for (int i = 0; i < 6; i++)
            {
                val[i] = para[i];
            }
            Type = type;
            Num++;
        }

    }


    public class bot
    {//TEST

        //test
        public Solution IkineResult;
        public double[] joint = { 0, -90, 0, 0, 90, 0 };

        public double[] ToolSetting = { 0, 0, 0, 0, 0, 0 };

        public double Threshold = 100;

        public double[] ToolPose = { 0, 0, 0, 0, 0, 0 };

        //public static readonly double[] Position_Difference;
        public double[] Initial_Position = { 0, -90, 0, 0, 90, 0 };
        public double[] Position_Difference = new double[6];
        public double[] ExPos = new double[6];
        //bot点到点运动
        public Cartesian SelectedCar = Cartesian.x;
        public Transfrom SelectedTrans//判断正负方向
        {
            get { return _SelectedTrans; }
            set
            {
                _SelectedTrans = value;
                if (value == Transfrom.Increase)
                { StepAngle = Math.Abs(StepAngle); StepDistance = Math.Abs(StepDistance); }
                else
                { StepAngle = -Math.Abs(StepAngle); StepDistance = -Math.Abs(StepDistance); }
            }
        }
        private Transfrom _SelectedTrans;
        public Transfrom RefCoor = Transfrom.World;
        public int SelectedJoint = 0;
        public double[] Test = new double[4];//[0]为速度，[1]为有效解数目,[2][3]为跳变判别
        public double[] JointUpperLimit { get; }
        public double[] JointLowerLimit { get; }
        public double StepAngle { get; set; }
        public double[] pose = { 0, 0, 0, 0, 0, 0 };
        public double[] solution = new double[48];
        public double StepDistance;
        public double[] TempPose = new double[6];
        #region 构造
        public bot()//构造函数无返回值
        {
            JointUpperLimit = new double[6] { 180f, 135f, 90f, 180f, 120f, 180f };
            JointLowerLimit = new double[6] { -179.99f, -135f, -90f, -179.99f, -120f, -179.99f };
            StepAngle = 0;

        }
        #endregion
        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "RotXYZ", CallingConvention = CallingConvention.Cdecl)]
        private static extern void RotXYZ(Cartesian xyz, double[] TempPose, double delta);
        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "inkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern Solution inkine(double[] p, double[] d, double[] solution, double[] joint, double Threshold, double[] speed);
        [DllImport("testInverseSolutionForRobotDll.dll", EntryPoint = "ToolIkine", CallingConvention = CallingConvention.Cdecl)]
        private static extern Solution ToolIkine(double[] p, double[] d, Cartesian xyz, double delta, double[] solution, double[] joint, double Threshold, double[] speed);
        public bool JointIncre()
        {
            bool OutOfRange = false;
            joint[SelectedJoint] += StepAngle;
            if (joint[SelectedJoint] > JointUpperLimit[SelectedJoint])
            {
                OutOfRange = true;
                joint[SelectedJoint] -= StepAngle;
            }
            return OutOfRange;
        }
        public bool JointDecre( )
        {
            bool OutOfRange = false;
            joint[SelectedJoint] += StepAngle;
            if (joint[SelectedJoint]< JointLowerLimit[SelectedJoint])
            {
                OutOfRange = true;
                joint[SelectedJoint] -= StepAngle;
            }
            return OutOfRange;
        }
        public void RollBack()
        {
            switch (SelectedCar)
            {
                case Cartesian.x: pose[0] -= StepDistance; break;
                case Cartesian.y: pose[1] -= StepDistance; break;
                case Cartesian.z: pose[2] -= StepDistance; break;

                case Cartesian.rot: pose[3] -= StepDistance; break;
                case Cartesian.pitch: pose[4] -= StepAngle; break;
                case Cartesian.yaw: pose[5] -= StepAngle; break;
            }


        }
        public void teach()
        {
            if (RefCoor == Transfrom.World)
            {
                for (int i = 0; i < 6; i++)
                {
                    TempPose[i] = pose[i];
                }
                switch (SelectedCar)
                {
                    case Cartesian.x: TempPose[0] += StepDistance; break;
                    case Cartesian.y: TempPose[1] += +StepDistance; break;
                    case Cartesian.z: TempPose[2] += StepDistance; break;
                    case Cartesian.rotx: RotXYZ(Cartesian.rotx, TempPose, StepAngle); break;
                    case Cartesian.roty: RotXYZ(Cartesian.roty, TempPose, StepAngle); break;
                    case Cartesian.rotz: RotXYZ(Cartesian.rotz, TempPose, StepAngle); break;
                    case Cartesian.rot: TempPose[3] += StepDistance; break;
                    case Cartesian.pitch: TempPose[4] += StepAngle; break;
                    case Cartesian.yaw: TempPose[5] += StepAngle; break;
                }
                IkineResult = inkine(TempPose, ToolSetting, solution, joint, Threshold, Test);
            }
            else if (RefCoor == Transfrom.Tool)
            {
                for (int i = 0; i < 6; i++)
                {
                    TempPose[i] = pose[i];
                }
                switch (SelectedCar)
                {
                    case Cartesian.x:
                    case Cartesian.y:
                    case Cartesian.z:
                        IkineResult = ToolIkine(TempPose, ToolSetting, SelectedCar, StepDistance, solution, joint, Threshold, Test);//获取逆解个数
                        break;
                    case Cartesian.rotx:
                    case Cartesian.roty:
                    case Cartesian.rotz:
                        IkineResult = ToolIkine(TempPose, ToolSetting, SelectedCar, StepAngle, solution, joint, Threshold, Test);//获取逆解个数
                        break;
                    default:break;
                }
                return;
            }
        }
        public void movebot(double[] Expected_Position,Timer wait)
        {

            ExPos = Expected_Position;
            for (int cnt = 0; cnt < 6; cnt++)
            { Position_Difference[cnt] = (Expected_Position[cnt] - joint[cnt]) / 100;
               //取3位有效数字 Position_Difference[cnt] = (double)Math.Round(Position_Difference[cnt], 3);
            }
           // OutputJoint(Position_Difference,"细分值");

            if (Compare_Position(joint, Expected_Position))
            { //Debug.WriteLine("关节角位置相同"); 
            }
            else
            {
               // Debug.WriteLine("开启");
                wait.Interval = 1;
                wait.Enabled = true;
            
            }
        }
        public void movebot(double[] Expected_Position, System.Timers.Timer wait)
        {

            ExPos = Expected_Position;
            for (int cnt = 0; cnt < 6; cnt++)
            { Position_Difference[cnt] = (Expected_Position[cnt] - joint[cnt]) /BotNode.NodeCount;

            }
             //Output_Joint(Position_Difference);

            if (Compare_Position(joint, Expected_Position))
            { Debug.WriteLine("关节角位置相同"); }
            else
            {
                Debug.WriteLine("开启wait1");
                wait.Interval =1;
                wait.Enabled = true;
            }
        }
        //判断是否是同一位置
        public bool Compare_Position(double[] Current_Position, double[] Expected_Position)
        {
            bool JointIsReset = true;
            for (int cnt = 0; cnt < 6; cnt++)
            {
                double error;
                error = Math.Abs(Current_Position[cnt] - Expected_Position[cnt]);
                if (error > 0.01)
                {
                    JointIsReset = false;
                    return JointIsReset;
                }

            }
            return JointIsReset;
        }
        private void OutputJoint(double[] joint,string tip)
        {
            string Output_Jvalue = tip;
            foreach (double Joint_value in joint)
            {
                Output_Jvalue =Output_Jvalue + "/" + Joint_value.ToString();
            }
            Debug.WriteLine(Output_Jvalue);
        }
    }


}
